import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
import VL53L0X     

class TofReader(Node):

    def __init__(self):
        super().__init__('tof_reader')
        self.tof1 = VL53L0X.VL53L0X(tca9548a_num=0, tca9548a_addr=0x70)
        self.tof2 = VL53L0X.VL53L0X(tca9548a_num=1, tca9548a_addr=0x70)
        self.tof1.open()
        self.tof2.open()
        self.tof1.start_ranging(VL53L0X.Vl53l0xAccuracyMode.BETTER)
        self.tof2.start_ranging(VL53L0X.Vl53l0xAccuracyMode.BETTER)
        self.publisher_1 = self.create_publisher(Range, '/cranebot/tof1', 10)
        self.publisher_2 = self.create_publisher(Range, '/cranebot/tof2', 10)
        self.tof1_init = (self.tof1.get_distance()+self.tof1.get_distance()+self.tof1.get_distance()+self.tof1.get_distance()+self.tof1.get_distance())/5
        self.tof2_init = (self.tof2.get_distance()+self.tof2.get_distance()+self.tof2.get_distance()+self.tof2.get_distance()+self.tof2.get_distance())/5
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
    
    def timer_callback(self):
        msg1 = Range()
        msg1.radiation_type = 2
        msg1.range = float(self.tof1.get_distance()-self.tof1_init)
        self.publisher_1.publish(msg1)
        msg2 = Range()
        msg2.radiation_type = 2
        msg2.range = float(self.tof2.get_distance()-self.tof2_init)
        self.publisher_2.publish(msg2)
        #self.get_logger().info(str(msg1.range)+str(msg2.range))

def main(args=None):
    rclpy.init(args=args)
    tof_reader = TofReader()
    rclpy.spin(tof_reader)
    tof_reader.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

